//$CC main.c -o ectest_qns -L /home/lee/igh-mc -lethercat
//scp ectest_qns root@192.168.58.1:/home/mc
#include "config_qn.h"

/****************************************************************************/
int ConfigPDO()
{
    
    /********************/
    printf("xenomai Configuring PDOs...\n");    
    domainServoOutput = ecrt_master_create_domain(master);
    if (!domainServoOutput) {
        return -1;
    }
    domainServoInput = ecrt_master_create_domain(master);
    if (!domainServoInput) {
        return -1;
    }
    printf("nei cun is ok\n");
    /********************/
    
    int n,i;
    printf("xenomai Creating slave0 configurations...\n");
    sc_cooldrive =
       ecrt_master_slave_config(master,CoolDrivePos0,CoolDrive);
    if (!sc_cooldrive ) {
        fprintf(stderr, "Failed to get slave0 configuration.\n");
        return -1;
    }
    else printf("get right slave0 config sc_cooldrive\n");
    
    i=0;
    do
    {
    	
    	sleep(1);    	
    	n=ecrt_slave_config_pdos(sc_cooldrive, EC_END, el2004_syncs);
    	i++;
    	if(i>5){
    		printf("slave0 configurations is outtime\n");
    		break;
    	}
    }
    while(n);
    printf("slave0 configurations is ok\n");
    /********************/
    printf("xenomai Creating slave1 configurations...\n");
    sc_cooldrive =
       ecrt_master_slave_config(master,CoolDrivePos1,CoolDrive);
    if (!sc_cooldrive ) {
        fprintf(stderr, "Failed to get slave1 configuration.\n");
        return -1;
    }
    else printf("get right slave1 config sc_cooldrive\n");
    
    i=0;
    do
    {
    	sleep(1);
    	n=ecrt_slave_config_pdos(sc_cooldrive, EC_END, slave_1_syncs);
    	i++;
    	if(i>5){
    		printf("slave1 configurations is outtime\n");
    		break;
    	}
    }
    while(n);
    printf("slave1 configurations is ok\n");
    
    return 0;
    
}   
    
    /*
    if (ecrt_slave_config_pdos(sc_cooldrive, EC_END, slave_0_syncs)) {
        fprintf(stderr, "Failed to configure slave0 PDOs.\n");
        return -1;
    }
    
    
    /********************/
    /********************
    printf("xenomai Creating slave1 configurations...\n");
    sc_cooldrive =
        ecrt_master_slave_config(master, CoolDrivePos1, CoolDrive);
    if (!sc_cooldrive) {
        fprintf(stderr, "Failed to get slave configuration.\n");
        return -1;
    }
    
    if (ecrt_slave_config_pdos(sc_cooldrive, EC_END, slave_1_syncs)) {
        fprintf(stderr, "Failed to configure PDOs.\n");
        return -1;
    }
    
    
    
    
}

/*****************************************************************************
 * Domain_reg
 ****************************************************************************/
int DomainReg(void)
{
	/********************/
    printf("xenomai Configuring PDOs...\n");    
    domainServoOutput = ecrt_master_create_domain(master);
    if (!domainServoOutput) {
        return -1;
    }
    domainServoInput = ecrt_master_create_domain(master);
    if (!domainServoInput) {
        return -1;
    }
    printf("nei cun is ok\n");
    /********************/
   

   /********************/
    if (ecrt_domain_reg_pdo_entry_list(domainServoOutput, domainServoOutput_regs)) {
        fprintf(stderr, "PDO entry registration failed!\n");
        return -1;
    }
    if (ecrt_domain_reg_pdo_entry_list(domainServoInput, domainServoInput_regs)) {
        fprintf(stderr, "PDO entry registration failed!\n");
        return -1;
    }

    fprintf(stderr, "Creating SDO requests...\n");

    // ecrt_slave_config_sdo8(sc_estun, 0x6060, 0, 8);
    // ecrt_slave_config_sdo8(sc_estun, 0x60C2, 1, 1);

    return 0;
}


/*****************************************************************************
 * Realtime task
 ****************************************************************************/

void rt_check_domain_state(void)
{
    ec_domain_state_t ds = {};
    ec_domain_state_t ds1 = {};
    //domainServoInput
    ecrt_domain_state(domainServoInput, &ds);
    if (ds.working_counter != domainServoInput_state.working_counter) {
        printf("domainServoInput: WC %u.\n", ds.working_counter);
    }
    if (ds.wc_state != domainServoInput_state.wc_state) {
        printf("domainServoInput: State %u.\n", ds.wc_state);
    }
    domainServoInput_state = ds;
    //domainServoOutput
    ecrt_domain_state(domainServoOutput, &ds1);
    if (ds1.working_counter != domainServoOutput_state.working_counter) {
        printf("domainServoOutput: WC %u.\n", ds1.working_counter);
    }
    if (ds1.wc_state != domainServoOutput_state.wc_state) {
        printf("domainServoOutput: State %u.\n", ds1.wc_state);
    }
    domainServoOutput_state = ds1;
}

/****************************************************************************/
/*****************************************************************************/

void check_master_state(void)
{
    ec_master_state_t ms;

    ecrt_master_state(master, &ms);

    if (ms.slaves_responding != master_state.slaves_responding) {
        printf("%u slave(s).\n", ms.slaves_responding);
    }
    if (ms.al_states != master_state.al_states) {
        printf("AL states: 0x%02X.\n", ms.al_states);
    }
    if (ms.link_up != master_state.link_up) {
        printf("Link is %s.\n", ms.link_up ? "up" : "down");
    }

    master_state = ms;
}

/*****************************************************************************/

void check_slave_config_states(void)
{
    ec_slave_config_state_t s;

    ecrt_slave_config_state(sc_cooldrive, &s);

    if (s.al_state != sc_cooldrive_state.al_state) {
        printf("AnaIn: State 0x%02X.\n", s.al_state);
    }
    if (s.online != sc_cooldrive_state.online) {
        printf("AnaIn: %s.\n", s.online ? "online" : "offline");
    }
    if (s.operational != sc_cooldrive_state.operational) {
        printf("AnaIn: %soperational.\n", s.operational ? "" : "Not ");
    }

    sc_cooldrive_state = s;
}

/*****************************************************************************/




int main(void)
{
    
    printf("hello world!\n");
    //sleep(1);
    master = ecrt_request_master(0);
    if (!master) {
        return -1;
    }
    printf("request_master sucess\n");
    check_master_state();
    //check_slave_config_states();
    ConfigPDO();
    
    printf("release_master\n");
    ecrt_release_master(master);
    pause();
    return 0;
}
